#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <iostream>

using namespace std;

double t_recent;
double total_distance; // 累计距离
void distanceCallback(const nav_msgs::Odometry::ConstPtr &msg)
{
  double dt;
  double ds;
  double v;

  dt = msg->header.stamp.toSec() - t_recent;
  t_recent = msg->header.stamp.toSec();
  v = msg->twist.twist.linear.x;

  if (dt > 1 || dt < 0)
  {
    dt = 0;
  }

  ds = v * dt;
  total_distance += ds;

  ROS_INFO("Linear velocity: v = %0.6f m/s, Total distance: s = %0.6f m", v, total_distance);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "distance_subscriber");
  ros::NodeHandle nh;
  total_distance = 0.0;
  t_recent = ros::Time::now().toSec();

  ROS_INFO("Initialized total distance: s = %0.6f m, t_recent = %0.6f s", total_distance, t_recent);

  ros::Subscriber distance_sub = nh.subscribe("/odom", 10, distanceCallback);
  ros::spin();

  return 0;
}